from machine import Pin, PWM
import time

"""
sg90 360度舵机
只能改变转速
0.5ms 正向最大速度
1.5ms 速度为0
2.5ms 反向最大速度
"""


class Servo:
    def __init__(self, pin_num):
        self.freq = 50
        self.pwm = PWM(Pin(pin_num), freq=self.freq, duty=0)

    def run(self, speed=1):
        """
        speed: 速度 [-100, +100]
        """
        if speed < -100 or speed > 100:
            print("speed between -100 and 100")
            return

        us = 1.5  # 默认不转动
        if speed > 0 and speed <= 100:
            us = 0.5 + 1 * (100 - speed) / 100

        if speed < 0:
            us = 1.5 - 1 * speed / 100

        # print(us)
        duty = int(us / 20 * 1023)
        self.pwm.duty(duty)

    def stop(self):
        self.pwm.deinit()


if __name__ == "__main__":
    servo = Servo(4)
    servo.run(100)
    time.sleep(3)
    servo.run(-100)
    time.sleep(3)
    servo.stop()
